// #include "pwm.h"
// #include "k210.h"
// #include "flash.h"
// #include "STMFCT.h"
// #include "task.h"
// #include "math.h"
// #include "OPENMV.h"
// #include "MPU6050.h"

// extern uint8_t Yao_Flag;
// extern int task_state;
// extern uint8_t numberfrom_k210;
// extern uint8_t Serial_RxData;
// extern uint8_t Serial_RxFlag;
// extern uint8_t RxData;
// // extern double angle;
// extern uint8_t distanceflag;
// extern uint64_t distance;

// u8 JHT_value = 1;
// u8 LAST_JHT_value = 1;
// uint16_t CountSensor_Count;
// uint8_t stageA1_notdone = 1;
// uint8_t motionfrom_openmv = 0;
// uint8_t stageB1_notdone = 1;
// uint8_t stageC1_notdone = 1;
// uint8_t stageD1_notdone = 1;
// uint8_t stageE1_notdone = 1;

// uint8_t stageA2_notdone = 1;
// uint8_t stageB2_notdone = 1;
// uint8_t stageC2_notdone = 1;
// uint8_t stageD2_notdone = 1;
// uint8_t stageE2_notdone = 1;
// uint8_t stageF2_notdone = 1;

// uint8_t stageA3_notdone = 1;
// uint8_t stageB3_notdone = 1;
// uint8_t stageC3_notdone = 1;
// uint8_t stageD3_notdone = 1;
// uint8_t stageE3_notdone = 1;
// uint8_t stageF3_notdone = 1;

// bool car_callibrate = false;
// bool car_turnleft = false;
// uint8_t left_number = 0;
// uint8_t rightflag = 1;
// uint8_t leftflag = 1;
// uint8_t left_180degreesflag = 1;

// void callibrate_setup()
// {
//     openmv_exist = true;
// }

// void callibrate_loop()
// {
//     PWM_L = PWM_R = 60;
//     PWM_L += 1 * OPENMV.xa;
//     PWM_R -= 1 * OPENMV.xa;
//     PWM_L += 0.5 * OPENMV.theta;
//     PWM_R -= 0.5 * OPENMV.theta;
//     if (PWM_L > 75)
//     {
//         PWM_L = 75;
//     }
//     if (PWM_R > 75)
//     {
//         PWM_R = 75;
//     }
//     if (PWM_L < 45)
//     {
//         PWM_L = 45;
//     }
//     if (PWM_R < 45)
//     {
//         PWM_R = 45;
//     }
//     TIM_L298N_BR(PWM_R);
//     TIM_L298N_AL(PWM_L);
// }

// int readmotionfrom_openmv(void)
// {
//     return 0;
// }

// void car_go(int8_t speed)
// {
//     TIM_L298N_AL(speed);
//     TIM_L298N_BR(speed);
// }

// void car_right(int8_t speed)
// {
//     TIM_L298N_AL(speed);
//     TIM_L298N_BR(-speed);
// }

// void car_left(int8_t speed)
// {
//     TIM_L298N_AL(-speed);
//     TIM_L298N_BR(speed);
// }

// void left_ninetydegrees(void)
// {
//     int16_t old_angle = MPU6050.yaw;
//     leftflag = 1;
//     while (leftflag == 1)
//     {
//         if (abs((int16_t)MPU6050.yaw - old_angle) <= 20 && leftflag == 1) // 20
//         {
//             car_left(78);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 20 && leftflag == 1 && abs((int16_t)angle - old_angle) <= 40) // 40
//         {
//             car_left(68);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 40 && leftflag == 1 && abs((int16_t)angle - old_angle) <= 60) // 60
//         {
//             car_left(65);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 60 && leftflag == 1 && abs((int16_t)angle - old_angle) <= 80) // 63
//         {
//             car_left(58);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 83 && leftflag == 1) // must convert angle to int16_t
//         {
//             car_left(0);
//             leftflag = 0;
//             MPU6050.yaw = 0;
//         }
//     }
// }

// void left_180degrees(void)
// {
//     int16_t old_angle = MPU6050.yaw;
//     left_180degreesflag = 1;
//     while (left_180degreesflag == 1)
//     {
//         if (abs((int16_t)MPU6050.yaw - old_angle) <= 40 && leftflag == 1) // 20
//         {
//             car_left(68);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 40 && leftflag == 1 && abs((int16_t)MPU6050.yaw - old_angle) <= 80) // 40
//         {
//             car_left(60);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 80 && leftflag == 1 && abs((int16_t)MPU6050.yaw - old_angle) <= 120) // 60
//         {
//             car_left(56);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 120 && leftflag == 1 && abs((int16_t)MPU6050.yaw - old_angle) <= 173) // 63
//         {
//             car_left(40);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 173 && leftflag == 1) // must convert angle to int16_t
//         {
//             car_left(0);
//             left_180degreesflag = 0;
//             MPU6050.yaw = 0;
//         }
//     }
// }

// void right_ninetydegrees(void)
// {
//     int16_t old_angle = MPU6050.yaw;
//     while (rightflag == 1)
//     {
//         if (abs((int16_t)MPU6050.yaw - old_angle) <= 20 && rightflag == 1) // 20
//         {
//             car_right(76);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 20 && rightflag == 1 && abs((int16_t)MPU6050.yaw - old_angle) <= 40) // 40
//         {
//             car_right(73);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 40 && rightflag == 1 && abs((int16_t)MPU6050.yaw - old_angle) <= 60) // 60
//         {
//             car_right(68);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 60 && rightflag == 1 && abs((int16_t)MPU6050.yaw - old_angle) <= 80) // 63
//         {
//             car_right(60);
//         }
//         if (abs((int16_t)MPU6050.yaw - old_angle) >= 83 && rightflag == 1) // must convert angle to int16_t
//         {
//             car_right(0);
//             rightflag = 0;
//         }
//     }
// }

// void Medicine(void) // detect medicine
// {
//     JHT_value = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4); // block high
//     if (LAST_JHT_value == 1 && JHT_value == 0)            // ����ҩƷ     there was one and there is not one
//     {
//         Yao_Flag = 0;
//     }
//     if (LAST_JHT_value == 0 && JHT_value == 1) // ����ҩƷ
//     {
//         Yao_Flag = 1;
//     }
//     LAST_JHT_value = JHT_value;
// }

// void stageA1(void)
// {
//     if (motionfrom_openmv == 1)
//     {
//         car_go(60);
//     }
//     if (motionfrom_openmv == 0)
//     {
//         car_go(60);
//         Delay_ms(100); // to be tested
//         car_go(0);
//         stageA1_notdone = 0;
//     }
// }

// void stageB1(void)
// {
//     if (numberfrom_k210 == 1) // number 1 room is on the left
//     {
//         left_ninetydegrees();
//     }
//     if (numberfrom_k210 == 2) // number 2 room is on the right
//     {
//         right_ninetydegrees();
//     }
//     stageB1_notdone = 0;
//     rightflag = 1;
//     leftflag = 1;
// }

// void stageC1(void)
// {
//     if (motionfrom_openmv == 1)
//     {
//         car_go(60);
//     }
//     if (motionfrom_openmv == 0)
//     {
//         car_go(0);
//         stageC1_notdone = 0;
//     }
// }

// void stageD1(void)
// {
//     if (Yao_Flag == 1)
//     {
//         car_go(0);
//     }
//     if (Yao_Flag == 0)
//     {
//         Delay_ms(1000);
//         distanceflag = 1;
//         if (distance <= 350)
//         {
//             car_go(55);
//         }
//         if (distance >= 350)
//         {
//             car_go(0);
//             distanceflag = 0;
//         }
//         stageD1_notdone = 0;
//     }
// }
// void stageE1(void)
// {
//     if (numberfrom_k210 == 1) // number 1 room is on the left
//     {
//         left_ninetydegrees();
//     }
//     if (numberfrom_k210 == 2) // number 2 room is on the right
//     {
//         right_ninetydegrees();
//     }
//     stageE1_notdone = 0;
//     rightflag = 1;
//     leftflag = 1;
// }

// void task1(void)
// {
//     if (stageA1_notdone == 1)
//     {
//         motionfrom_openmv = readmotionfrom_openmv();
//         stageA1(); // go straight until told by openmv
//     }
//     if (stageA1_notdone == 0 && stageB1_notdone == 1)
//     {
//         stageB1(); // turn right or left for 90 degrees
//     }
//     if (stageA1_notdone == 0 && stageB1_notdone == 0 && stageC1_notdone == 1)
//     {
//         stageC1(); // go straight until told by openmv
//     }
//     if (stageA1_notdone == 0 && stageB1_notdone == 0 && stageC1_notdone == 0 && stageD1_notdone == 1)
//     {
//         stageD1(); // waiting to be discharged and reverse
//     }
//     if (stageA1_notdone == 0 && stageB1_notdone == 0 && stageC1_notdone == 0 && stageD1_notdone == 0 && stageE1_notdone == 1)
//     {
//         stageE1(); // turn right or left for 90 degrees
//     }
//     if (stageA1_notdone == 0 && stageB1_notdone == 0 && stageC1_notdone == 0 && stageD1_notdone == 0 && stageE1_notdone == 0)
//     {
//         car_go(60);
//         Delay_ms(1500); // to be tested
//         car_go(0);
//         task_state = 0; // remember even if the power is off
//         int array1[1] = {2};
//         FLASH_WriteByte(0x08011000, (uint64_t *)&array1, 1);
//         int array2[1] = {8};
//         FLASH_WriteByte(0x0801FC00, (uint64_t *)&array2, 1);
//     }
// }

// void stageB2(void)
// {
//     if (motionfrom_openmv == 1)
//     {
//         car_go(60);
//     }
//     if (motionfrom_openmv == 0)
//     {
//         car_go(60);
//         Delay_ms(100); // to be tested
//         car_go(0);
//         stageB2_notdone = 0;
//     }
// }

// void stageC2(void)
// {
//     while (!left_number)
//     {
//         left_number = K210.leftnumber1;
//     }
//     if (numberfrom_k210 == left_number)
//     {
//         left_ninetydegrees();
//         stageC2_notdone = 0;
//         leftflag = 1;
//     }
//     if (numberfrom_k210 != left_number)
//     {
//         right_ninetydegrees();
//         stageC2_notdone = 0;
//         rightflag = 1;
//     }
// }
// void stageD2(void)
// {
//     if (motionfrom_openmv == 1)
//     {
//         car_go(60);
//     }
//     if (motionfrom_openmv == 0)
//     {
//         car_go(0);
//         stageD2_notdone = 0;
//     }
// }

// void stageE2(void)
// {
//     if (Yao_Flag == 1)
//     {
//         car_go(0);
//     }
//     if (Yao_Flag == 0)
//     {
//         Delay_ms(1000);
//         car_go(-60);
//         Delay_ms(2000); // to be tested
//         stageE2_notdone = 0;
//     }
// }

// void stageF2(void)
// {
//     if (numberfrom_k210 == left_number) // number 1 room is on the left
//     {
//         left_ninetydegrees();
//     }
//     if (numberfrom_k210 != left_number) // number 2 room is on the right
//     {
//         right_ninetydegrees();
//     }
//     stageF2_notdone = 0;
//     rightflag = 1;
//     leftflag = 1;
// }

// void task2(void)
// {
//     if (stageA2_notdone == 1)
//     {
//         car_go(60);     // go straight until it passes the first crossing
//         Delay_ms(2000); // to be tested
//         stageA2_notdone = 0;
//     }
//     if (stageA2_notdone == 0 && stageB2_notdone == 1)
//     {
//         motionfrom_openmv = readmotionfrom_openmv();
//         stageB2(); // go straight until told by openmv
//     }
//     if (stageA2_notdone == 0 && stageB2_notdone == 0 && stageC2_notdone == 1)
//     {
//         stageC2(); // turn right or left told by k210
//     }
//     if (stageA2_notdone == 0 && stageB2_notdone == 0 && stageC2_notdone == 0 && stageD2_notdone == 1)
//     {
//         stageD2(); // go straight until told by openmv
//     }
//     if (stageA2_notdone == 0 && stageB2_notdone == 0 && stageC2_notdone == 0 && stageD2_notdone == 0 && stageE2_notdone == 1)
//     {
//         stageE2(); // waiting to be discharged and reverse
//     }
//     if (stageA2_notdone == 0 && stageB2_notdone == 0 && stageC2_notdone == 0 && stageD2_notdone == 0 && stageE2_notdone == 0 && stageF2_notdone == 1)
//     {
//         stageF2(); // waiting to be discharged and reverse
//     }
//     if (stageA2_notdone == 0 && stageB2_notdone == 0 && stageC2_notdone == 0 && stageD2_notdone == 0 && stageE2_notdone == 0 && stageF2_notdone == 0)
//     {
//         car_go(60);
//         Delay_ms(2000); // to be tested
//         car_go(0);
//         task_state = 0; // remember even if the power is off
//         int array1[1] = {3};
//         FLASH_WriteByte(0x08011000, (uint64_t *)&array1, 1);
//         int array2[1] = {8};
//         FLASH_WriteByte(0x0801FC00, (uint64_t *)&array2, 1);
//     }
// }

// void stageB3(void)
// {
//     if (motionfrom_openmv == 1)
//     {
//         car_go(60);
//     }
//     if (motionfrom_openmv == 0)
//     {
//         car_go(60);
//         Delay_ms(100); // to be tested
//         car_go(0);
//         stageB2_notdone = 0;
//     }
// }

// void task3(void)
// {
//     if (stageA3_notdone == 1)
//     {
//         car_go(60);     // go straight until it passes the first and the second crossing
//         Delay_ms(2000); // to be tested
//         stageA3_notdone = 0;
//     }
//     if (stageA3_notdone == 0 && stageB3_notdone == 1)
//     {
//         motionfrom_openmv = readmotionfrom_openmv();
//         stageB3(); // go straight until told by openmv
//     }
//     if (stageA3_notdone == 0 && stageB3_notdone == 0 && stageC3_notdone == 1)
//     {
//         stageC2(); // turn right or left told by k210
//     }
//     if (stageA3_notdone == 0 && stageB3_notdone == 0 && stageC3_notdone == 0 && stageD3_notdone == 1)
//     {
//         stageD2(); // go straight until told by openmv
//     }
//     if (stageA3_notdone == 0 && stageB3_notdone == 0 && stageC3_notdone == 0 && stageD3_notdone == 0 && stageE3_notdone == 1)
//     {
//         stageE2(); // waiting to be discharged and reverse
//     }
//     if (stageA3_notdone == 0 && stageB3_notdone == 0 && stageC3_notdone == 0 && stageD3_notdone == 0 && stageE3_notdone == 0 && stageF3_notdone == 1)
//     {
//         stageF2(); // waiting to be discharged and reverse
//     }
//     if (stageA3_notdone == 0 && stageB3_notdone == 0 && stageC3_notdone == 0 && stageD3_notdone == 0 && stageE3_notdone == 0 && stageF3_notdone == 0)
//     {
//         car_go(60);
//         Delay_ms(2000); // to be tested
//         car_go(0);
//         task_state = 0; // remember even if the power is off
//         int array1[1] = {1};
//         FLASH_WriteByte(0x08011000, (uint64_t *)&array1, 1);
//         int array2[1] = {8};
//         FLASH_WriteByte(0x0801FC00, (uint64_t *)&array2, 1);
//     }
// }

// ////���ȶ���PID�ṹ�����ڴ��һ��PID������
// //  typedef struct
// // {
// //    float kp,ki,kd;//����ϵ��
// //    float error,lastError;//���ϴ����
// //    float integral,maxIntegral;//���֡������޷�
// //    float output,maxOutput;//���������޷�
// //}PID;
// //
// ////���ڳ�ʼ��pid�����ĺ���
// // void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut)
// //{
// //     pid->kp=p;
// //     pid->ki=i;
// //     pid->kd=d;
// //     pid->maxIntegral=maxI;
// //     pid->maxOutput=maxOut;
// // }
// //
// ////����һ��pid����
// ////����Ϊ(pid�ṹ��,Ŀ��ֵ,����ֵ)������������pid�ṹ���output��Ա��
// // void PID_Calc(PID *pid,float reference,float feedback)
// //{
// //   //��������
// //     pid->lastError=pid->error;//����error������
// //     pid->error=reference-feedback;//������error
// //     //����΢��
// //     float dout=(pid->error-pid->lastError)*pid->kd;
// //     //�������
// //     float pout=pid->error*pid->kp;
// //     //�������
// //     pid->integral+=pid->error*pid->ki;
// //     //�����޷�
// //     if(pid->integral > pid->maxIntegral) pid->integral=pid->maxIntegral;
// //     else if(pid->integral < -pid->maxIntegral) pid->integral=-pid->maxIntegral;
// //     //�������
// //     pid->output=pout+dout+pid->integral;
// //     //����޷�
// //     if(pid->output > pid->maxOutput) pid->output=pid->maxOutput;
// //     else if(pid->output < -pid->maxOutput) pid->output=-pid->maxOutput;
// // }
// //
// // PID mypid;//����һ��PID�ṹ�����
// //
// ///*int main()
// //{
// //    //...������Щ������ʼ������
// //    PID_Init(&mypid,10,1,5,800,1000);//��ʼ��PID����
// //    while(1)//����ѭ������
// //    {
// //        float feedbackValue=...;//�����ȡ�����ض���ķ���ֵ
// //        float targetValue=...;//�����ȡ��Ŀ��ֵ
// //        PID_Calc(&mypid,targetValue,feedbackValue);//����PID���㣬�����output��Ա������
// //        //�趨ִ���������С(mypid.output);
// //        delay(10);//�ȴ�һ��ʱ���ٿ�ʼ��һ��ѭ��
// //    }
// //}
// //*/
